#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "imu.h"
#include "pid.h"
#include "Motor.h"
#include "encoder.h"
#include "MPU6050.h"
#include "FreeRTOS.h"
#include "task.h"


#define START_TASK_PRIO		1	
#define START_STK_SIZE 		128  
TaskHandle_t StartTask_Handler;
void start_task(void *pvParameters);

#define IMU_TASK_PRIO		2	
#define IMU_STK_SIZE 		50  
TaskHandle_t ImuTask_Handler;
void imu_task(void *pvParameters);


#define LED1_TASK_PRIO		3
#define LED1_STK_SIZE 		50  
TaskHandle_t LED1Task_Handler;
void led1_task(void *pvParameters);
float Med_Angle = 0;
float Target_Speed = 0;
int Vertical_out,Velocity_out,Turn_out;
float Velocity_p = 0,Velocity_i = 0,Velocity_d = 0;
float Vertical_p =1000,Vertical_i = 0,Vertical_d = 0;
static MPU6050_Data mpu_data;
struct pid Pid[2];
int main(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); 
	delay_init();	    			
	uart_init(115200);				
	LED_Init();	
    Motor_Init();
	Encoder_Init();
	Encoder2_Init();	
    MPU6050_Init();  
	pid_init(&Pid[0]);//��??pid?????
	pid_init(&Pid[1]);
	pid_set(&Pid[0],Velocity_p,Velocity_i,Velocity_d);
	pid_set(&Pid[1],Vertical_p,Vertical_i,Vertical_d);	
    // mpu6050_calbration();
    // delay_ms(2000);			
	printf("main start\r\n");

    xTaskCreate((TaskFunction_t )start_task,           
                (const char*    )"start_task",         
                (uint16_t       )START_STK_SIZE,       
                (void*          )NULL,                 
                (UBaseType_t    )START_TASK_PRIO,      
                (TaskHandle_t*  )&StartTask_Handler);   
    vTaskStartScheduler();         
}


void start_task(void *pvParameters)
{
    taskENTER_CRITICAL();           
    xTaskCreate((TaskFunction_t )imu_task,     	
                (const char*    )"imu_task",   	
                (uint16_t       )IMU_STK_SIZE, 
                (void*          )NULL,				
                (UBaseType_t    )IMU_TASK_PRIO,	
                (TaskHandle_t*  )&ImuTask_Handler);   
    xTaskCreate((TaskFunction_t )led1_task,     
                (const char*    )"led1_task",   
                (uint16_t       )LED1_STK_SIZE, 
                (void*          )NULL,
                (UBaseType_t    )LED1_TASK_PRIO,
                (TaskHandle_t*  )&LED1Task_Handler);         
    vTaskDelete(StartTask_Handler); 
    taskEXIT_CRITICAL();            
}




void led1_task(void *pvParameters)
{
    while(1)
    {
        int16_t Encoder_left = Encoder1GetSpeed();
		int16_t Encoder_right = Encoder2GetSpeed();       
		// uint8_t ID = MPU6050_GetID();
		// printf("ID:%d\n",ID);
		Motor_SetSpeed(200);
		Motor_SetSpeed1(200);
        mpu6050_get_data(&mpu_data);
		Velocity_out=Velocity(&Pid[0],Target_Speed,Encoder_left,Encoder_right);
        IMU_data* imu_data = {0};
        imu_data = imu_get_data();
        Vertical_out = Vertical(&Pid[1],Med_Angle,imu_data->roll,mpu_data.gyro_y);	
        printf("roll:%d  gyro_y:%d  Vertical_out:%d\n",(int16_t)imu_data->roll,(int16_t)mpu_data.gyro_y,(int16_t)Vertical_out);		  
//		Motor_SetSpeed(Vertical_out);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      (95);
//		Motor_SetSpeed1(Vertical_out);
        vTaskDelay(500);
    }
}
